package edu.uci.ics.ai.maze.algorithms;

import java.util.HashSet;
import java.util.Set;
import java.util.LinkedList;
import java.util.ArrayList;
import edu.uci.ics.ai.maze.controllers.SearchContext;
import java.util.List;
import java.util.ArrayDeque;
import edu.uci.ics.ai.maze.models.SearchAlgorithm;
import edu.uci.ics.ai.maze.models.Vertex;
import java.util.Queue;
import static edu.uci.ics.ai.maze.utils.MazeUtils.euclideanDistance;

/**
 * Implementation of breadth first search.
 */
public class BreadthFirstSearch implements SearchAlgorithm {

  private Set<Vertex> visited;
  private Queue<Node> frontier;

  @Override
  public String getName() {
    return "Breadth First Search";
  }

  @Override
  public List<Vertex> calculateReturnPath(SearchContext context) {
    // test for case that there is no goal in maze
    if (context.getGoal() == null) {
      return new ArrayList<Vertex>(0);
    } else {
      return bfs(context);
    }
  }

  private List<Vertex> bfs(SearchContext context) {
    visited = new HashSet<Vertex>(context.getVertexCount());
    frontier = new ArrayDeque<Node>(context.getVertexCount());
    frontier.offer(new Node(context.getStart(), null, 0));

    Node curNode = null;
    while (!frontier.isEmpty()) {
      curNode = frontier.poll();
      if (curNode.equals(context.getGoal())) {
        break;
      }
      expandMerge(curNode, context);
    }

    // traverse the node tree to build up the node path from start
    // to finish
    LinkedList<Vertex> path = new LinkedList<Vertex>();
    while (curNode != null) {
      path.addFirst(curNode);
      curNode = curNode.getParent();
    }

    return path;
  }

  private void expandMerge(Node curNode, SearchContext context) {
    //first check for a direct path to goal, if so, just add
    if (context.isVisible(curNode, context.getGoal())) {
      processVertex(context.getGoal(), curNode);
    } else { // else, find all visible nodes
      for (Vertex child : context.getVertices()) {
        if (!visited.contains(child)) {
          processVertex(child, curNode);
        }
      }
    }
  }

  private void processVertex(Vertex v, Node parent) {
    visited.add(v);
    frontier.offer(new Node(v, parent, parent.getGn() + euclideanDistance(parent, v)));
  }

  private class Node extends Vertex {

    /** the distance from the start node */
    private final double gn;
    private final Node parent;
    
    private Node(Vertex v, Node parent, double gn) {
      super(v);
      this.parent = parent;
      this.gn = gn;
    }

    public Node getParent() {
      return parent;
    }

    public double getGn() {
      return gn;
    }

  }
}
